//
// Created by LiuDongPeng on 2023/4/21.
//

#include "as5600.h"



static int as5600_init(as5600_t* dev);


/**
 * @brief Init AS5600 device
 * @param[in]   dev
 * @return
 */
static int as5600_init(as5600_t* dev)
{
    return 0;
}



/**
 * @brief Create AS5600 object
 * @param[out]   dev
 * @param[in]   i2c_mem_w_cb
 * @param[in]   i2c_mem_r_cb
 * @return
 */
int as5600_create(as5600_t* dev, i2c_mem_write_t i2c_mem_w_cb, i2c_mem_read_t i2c_mem_r_cb)
{
    if (dev == NULL || i2c_mem_r_cb == NULL)
        return -1;

    dev->i2c_mem_write_cb = i2c_mem_w_cb;
    dev->i2c_mem_read_cb = i2c_mem_r_cb;

    return as5600_init(dev);
}


/**
 * @brief Get raw angle data
 * @param[in]   dev
 * @return
 */
int16_t as5600_get_raw_angle(as5600_t* dev)
{
    int16_t val = 0;
    uint8_t msb = 0, lsb = 0;

    if (dev == NULL || dev->i2c_mem_read_cb == NULL)
        return -1;

    dev->i2c_mem_read_cb(AS5600_I2C_ADDRESS, RAW_ANGLE_MSB_REG, 1, &msb, 1, 1000);
    dev->i2c_mem_read_cb(AS5600_I2C_ADDRESS, RAW_ANGLE_LSB_REG, 1, &lsb, 1, 1000);

    val = lsb | (msb << 8);

    return val;
}

/**
 * @brief Get angle data
 * @param[in]   dev
 * @return
 */
float as5600_get_angle(as5600_t* dev)
{
    float angle = 0;
    int16_t raw = 0;

    raw = as5600_get_raw_angle(dev);
    angle = 360.0 / 4095.0 * raw;

    return angle;
}